#ifndef FLASHROS_FUSE_ROS_CACHE_UTILS_H
#define FLASHROS_FUSE_ROS_CACHE_UTILS_H

#include <message_filters/cache.h>
#include <message_filters/subscriber.h>
#include <ros/ros.h>

template <class T>
class RosCacheHelper {
 public:
  RosCacheHelper() {}
  void Init(ros::NodeHandle& nh, std::string topic_name,
            int subscriber_buffer_size = 1, int cache_buffer_size = 1) {
    sub_ = new message_filters::Subscriber<T>(nh, topic_name,
                                              subscriber_buffer_size);
    cache_ = new message_filters::Cache<T>(*sub_, cache_buffer_size);
    cache_->registerCallback(Callback);
  }

  void Callback(boost::shared_ptr<T const>& msg) {}

  boost::shared_ptr<T const>& msg GetLatextMsg() {
    cache_->getElemAfterTime(start_time_);
  }

  void Release() {
    delete sub_;
    delete cache_;
  }

 private:
  ros::Time start_time_ = ros::Time::now();
  message_filters::Cache<T>* cache_;
  message_filters::Subscriber<T>* sub_;
};

#endif  // FLASHROS_FUSE_ROS_CACHE_UTILS_H